#include <ros/ros.h>
#include "executor/chassis_executor.h"
#include "executor/gimbal_executor.h"
#include "behavior_tree/behavior_node.h"
#include "behavior_tree/behavior_tree.h"
// #include "example_behavior/aim_behavior.h"
#include "example_behavior/back_boot_area_behavior.h"
#include "example_behavior/chase_behavior.h"
#include "example_behavior/circle_behavior.h"
#include "example_behavior/dance_behavior.h"
#include "example_behavior/escape_behavior.h"
#include "example_behavior/gimbal_aim_behavior.h"
#include "example_behavior/gimbalget0_behavior.h"
#include "example_behavior/goal_behavior.h"
#include "example_behavior/gogoal_behavior.h"
#include "example_behavior/patrol_behavior.h"
#include "example_behavior/search_behavior.h"
#include "example_behavior/shake_behavior.h"
#include "example_behavior/wait_behavior.h"
#include "example_behavior/bfs_search.h"
#include "example_behavior/buff_behavior.h"
#include "example_behavior/attack_behavior.h"

void param_init(ros::NodeHandle& p){}

int main(int argc, char **argv){
    ros::init(argc, argv, "tree_used_node");
	std::string full_path = ros::package::getPath("roborts_decision") + "/config/decision.prototxt";
	ros::NodeHandle nh_p("~") ;
	param_init(nh_p);
    auto blackboard_ptr_ = std::make_shared<roborts_decision::Blackboard>(full_path);
    roborts_decision::Blackboard* blackboard = &*blackboard_ptr_;
    //blackboard_ptr_->TurnMyId(blackboard_ptr_->GetRobotStatusinfo().id);//1 2 3 4
    auto bfs_search = new roborts_decision::BfsSearch(blackboard);
    auto chassis_executor_ptr_ = std::make_shared<roborts_decision::ChassisExecutor>();
    chassis_executor_ptr_->Set_Search(bfs_search);//
    auto gimbal_executor_ptr_ = std::make_shared<roborts_decision::GimbalExecutor>();
    auto attack_action_=std::make_shared<roborts_decision::AttackAction>(blackboard_ptr_);
    auto shake_action_= std::make_shared<roborts_decision::ShakeAction>(blackboard_ptr_, gimbal_executor_ptr_);
    //auto circle_action_ = std::make_shared<roborts_decision::CircleAction>(blackboard_ptr_,chassis_executor_ptr_);
    //auto buff_action_=std::make_shared<roborts_decision::BuffAction>(blackboard_ptr_, chassis_executor_ptr_ );
    auto dance_action_=std::make_shared<roborts_decision::DanceAction>(blackboard_ptr_,  chassis_executor_ptr_,  gimbal_executor_ptr_);
    auto attack_parallel_=std::make_shared<roborts_decision::ParallelNode>("attack parallel", blackboard_ptr_, 1);
    attack_parallel_->AddChildren(dance_action_);
    attack_parallel_->AddChildren(attack_action_);
    /*auto game_start_condition_=std::make_shared<roborts_decision::PreconditionNode>("game start condition", blackboard_ptr_, [&](){
        if(blackboard_ptr_->GetMyBulletSupplyBuffInfo())return true;
        else return false;
    }, roborts_decision::AbortType::BOTH);
    game_start_condition_->SetChild(attack_action_);*/
    ros::service::waitForService("cmd_fric_wheel");
    ros::NodeHandle n;
    ros::ServiceClient client = n.serviceClient<roborts_msgs::FricWhl>("cmd_fric_wheel");
    roborts_msgs::FricWhl Fric;
    Fric.request.open = true;
    if(!client.call(Fric))
        ROS_ERROR("FRICWHEEL NOT OPEN");

    auto attack_condition_=std::make_shared<roborts_decision::PreconditionNode>("attack condition", blackboard_ptr_, [&](){
        if(blackboard_ptr_->attack_condition())return true;
        else return false;
    }, roborts_decision::AbortType::BOTH);
    attack_condition_->SetChild(attack_parallel_);

    auto dance_selector_=std::make_shared<roborts_decision::SelectorNode>("dance selector",blackboard_ptr_);
   // dance_selector_->AddChildren(underattack_fivesec_condition_);
    dance_selector_->AddChildren(attack_condition_);
    dance_selector_->AddChildren(dance_action_);

    auto dance_condition_=std::make_shared<roborts_decision::PreconditionNode>("dance condition", blackboard_ptr_, [&](){
        if(blackboard_ptr_->GetIfFindEnemy())return true;//if enemy +1s
        else return false;
    }, roborts_decision::AbortType::BOTH);
    dance_condition_->SetChild(dance_selector_);

    auto root_selector_ = std::make_shared<roborts_decision::SelectorNode>("root",blackboard_ptr_);
    root_selector_->AddChildren(dance_condition_);
    root_selector_->AddChildren(shake_action_);
    roborts_decision::BehaviorTree root(root_selector_,10);

ros::service::waitForService("cmd_fric_wheel");
    ros::NodeHandle n;
    ros::ServiceClient client = n.serviceClient<roborts_msgs::FricWhl>("cmd_fric_wheel");
    roborts_msgs::FricWhl Fric;
    Fric.request.open = true;
    if(!client.call(Fric))
        ROS_ERROR("FRICWHEEL NOT OPEN");
    sleep(3);

    root.Run();
    return 0;
}

